/*************************************************************************
Purpose:  This program tests various ellipsoid functions.
************************************************************************** 
Input: None.
**************************************************************************
Output: Displays coordinates, that's all.
**************************************************************************   
Copyright 2010 Bryan Killett

This file is part of GAIA.

GAIA is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

GAIA is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with GAIA. If not, see <http://www.gnu.org/licenses/>.
*************************************************************************/
//Header file "definitions.hpp" declares many functions, constants, datatypes.
//In order to use the functions declared in definitions.hpp, must link
//with object code that includes functions.cpp.
#include "definitions.hpp"

int main(){

  double x,y,z,lat,lon,height;
  long long i,j,imax=5,jmax=4;
  char s[max_length];
  string temp_string;
  FILE *conversion_output;
  char* f_return;//Used to stop fgets return value warning.
  double x2,y2,z2;
  double lats,lons,xs,ys,zs,error,max_error=-1000000;
  if(1==2){
    cout<<"/////////////////////////////////////////////////////////////////////"<<endl;
    cout<<"//Conversion from Lat/Lon/Height to XYZ."<<endl;
    cout<<"/////////////////////////////////////////////////////////////////////"<<endl;
    for(i=0;i<imax;i++){
      for(j=0;j<jmax;j++){

        lat = 90.0-180.0*((double)i/((double)imax-1.0));
        lon = 360.0*((double)j/((double)jmax-1.0));
        height = 1000;
        //cout<<"lat: "<<lat<<", lon: "<<lon<<", height: "<<height<<endl;
        llh2xyz(x,y,z,lat,lon,height);
        //cout<<"New: x: "<<x<<", y: "<<y<<", z: "<<z<<endl;

        //Convert grid lat/lon/elev to x,y,z.
        sprintf(s,"%f %f %f",lat,lon,height);
        temp_string = gaiafolder();
        temp_string.append("software/gd2xyz.e ");
        temp_string += s;
        //cout << "The commmand used to convert the grid lat,lon,height to x,y,z is: " << endl << temp_string << endl;
        conversion_output = popen(temp_string.c_str(),"r");
        f_return=fgets(s,max_length,conversion_output);
        pclose(conversion_output);
        sscanf(s,"%lf %lf %lf",&x2,&y2,&z2);
        //cout<<"Old: x: "<<x2<<", y: "<<y2<<", z: "<<z2<<endl;
        xs = x-x2;
        ys = y-y2;
        zs = z-z2;
        //cout<<"Error: x: "<<xs<<", y: "<<ys<<", z: "<<zs<<endl;
        error = sqrt(xs*xs + ys*ys + zs*zs);
        //cout<<"Error: "<<error<<endl;
        if(error > max_error){
          max_error = error;
          lats = lat; lons = lon;
        }
      }
    }
    cout<<"GEOID_f: "<<setprecision(20)<<GEOID_f<<endl;
    cout<<"1/GEOID_f: "<<1.0/GEOID_f<<endl;
    cout<<"Max error of "<<setprecision(7)<<max_error<<" happened at lat "<<lats<<" and lon "<<lons<<endl<<endl;
  }
  if(1==1){
    imax=5; jmax=4;
    cout<<"/////////////////////////////////////////////////////////////////////"<<endl;
    cout<<"//Conversion from XYZ to Lat/Lon/Height."<<endl;
    cout<<"/////////////////////////////////////////////////////////////////////"<<endl;
    for(i=0;i<imax;i++){
      for(j=0;j<jmax;j++){

        lat = 90.0-180.0*((double)i/((double)imax-1.0));
        lon = 360.0*((double)j/((double)jmax-1.0));
        height = 0;
        llh2xyz(x,y,z,lat,lon,height);
        //cout<<"Input: x: "<<x<<", y: "<<y<<", z: "<<z<<endl;
        xyz2llh(x,y,z,lat,lon,height);
        //cout<<"New: lat: "<<lat<<", lon: "<<lon<<", height: "<<height<<endl;
        
        //Transform midpoint to lat,lon,alt to compare to input.
        double wlon,latm,lonm,altm; 
        sprintf(s,"%f %f %f",x,y,z);
        temp_string = gaiafolder();
        temp_string.append("software/xyz2gd.e ");
        temp_string += s;
        //cout<<"The commmand used to convert the xyz coordinates of the midpoint to geodetic coordinates like those in the input file is: "<<endl<<temp_string<<endl;
        conversion_output = popen(temp_string.c_str(),"r");
        f_return=fgets(s,max_length,conversion_output); 
        pclose(conversion_output);
        sscanf(s,"%lf %lf %lf %lf",&latm,&lonm,&altm,&wlon);
        
        //cout<<"Old: lat: "<<latm<<", lon: "<<lonm<<", height: "<<altm<<endl;
        double km_error = 110*180/pi*acos(cos((90-lat)*pi/180)*cos((90-latm)*pi/180) + sin((90-lat)*pi/180)*sin((90-latm)*pi/180)*cos((lon-lonm)*pi/180));

        cout<<"Surface error: "<<km_error<<", height error: "<<height-altm<<endl; 
      }
    }
  }

  return 0;
}
